(** Matrix manipulation *)

open Bigarray;;
open Vec;;

type matrix4 = (float, Bigarray.float32_elt, Bigarray.c_layout) Bigarray.Array1.t;;

(** Create a matrix. Contents are uninitialized. *)
let make () =
  Array1.create float32 c_layout 16;;

(** Copy a matrix. *)
let copy m = 
  let ret = make () in
    Bigarray.Array1.blit m ret;
    ret;;

(** Make an identity matrix *)
let identity () = 
  let ret = make () in
    Array1.fill ret 0.0;
    ret.{0} <- 1.0;
    ret.{5} <- 1.0;
    ret.{10} <- 1.0;
    ret.{15} <- 1.0;
    ret;;

(** Fill a matrix with 0.0 *)
let clear m = 
  Array1.fill m 0.0;
  m;;

(** Return a 4x4 roatation matrix from axis/angle *)
let axisAngle axis angle = 
  let ret = clear (make ()) in
  let c = cos angle in
  let s = sin angle in 
  let t = 1. -. (cos angle) in
    ret.{0} <- t *. axis.x *. axis.x +. c;
    ret.{1} <- t *. axis.x *. axis.y +. axis.z *. s;
    ret.{2} <- t *. axis.x *. axis.z -. axis.y *. s;
    ret.{4} <- t *. axis.x *. axis.y -. axis.z *. s;
    ret.{5} <- t *. axis.y *. axis.y +. c;
    ret.{6} <- t *. axis.y *. axis.z +. axis.x *. s;
    ret.{8} <- t *. axis.x *. axis.z +. axis.y *. s;
    ret.{9} <- t *. axis.y *. axis.z -. axis.x *. s;
    ret.{10}<- t *. axis.z *. axis.z +. c;
    ret.{15}<- 1.0;
    ret;;

(** Calculate a x b *)
let mult a b =
  let ret = make () in
    for x = 0 to 3 do
      for y = 0 to 3 do
	ret.{y*4+x} <- a.{y*4} *. b.{x} +. a.{y*4+1} *. b.{x+4} +. a.{y*4+2} *. b.{x+8} +. a.{y*4+3} *. b.{x+12};
      done;
    done;
    ret;;

(** Extract the z axis from the given matrix *)	  
let forward m =
  { x = m.{8}; y = m.{9}; z = m.{10} };;

(** Extract the x axis from the given matrix *)
let right m = 
  { x = m.{0}; y = m.{1}; z = m.{2} };;

(** Extract the y axis from the given matrix *)
let up m = 
  { x = m.{4}; y = m.{5}; z = m.{6} };;

(** Move the given matrix along its z axis 
    @param m Matrix
    @param d The distance to move *)
let moveForward m d =
  m.{12} <- m.{12} +. m.{8} *. d;
  m.{13} <- m.{13} +. m.{9} *. d;
  m.{14} <- m.{14} +. m.{10} *. d;
  m;;

    
(** Translate a matrix (in-place) by a vector 
    @param a The matrix
    @param v The vector *)
let xlate a v = 
  a.{12} <- a.{12} +. v.x;
  a.{13} <- a.{13} +. v.y;
  a.{14} <- a.{14} +. v.z;;

let print m = 
 Printf.printf "%.2f %.2f %.2f %.2f\n" m.{0} m.{4} m.{8} m.{12};
 Printf.printf "%.2f %.2f %.2f %.2f\n" m.{1} m.{5} m.{9} m.{13};
 Printf.printf "%.2f %.2f %.2f %.2f\n" m.{2} m.{6} m.{10} m.{14};
 Printf.printf "%.2f %.2f %.2f %.2f\n" m.{3} m.{7} m.{11} m.{15};;




